#include "pure_pursuit_core.h"
#include <geometry_msgs/TwistStamped.h>
#include <std_msgs/Float64.h>
#include "ros/ros.h"
#include "pure_pursuit_pkg/pure_pursuit_debug.h"
#include "cmath"
class ControlCmdWrapper:public auto_msgs::control_cmd
{
    
};
constexpr double LOOP_RATE =50;//processing frequency
constexpr double max_steering_wheel_angle =700;
constexpr double STEERING_RATIO = 18;
 int main(int argc, char **argv)
{
  
  // set up ros
  ros::init(argc, argv, "pure_pursuit");

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  pure_pursuit_pkg::PurePursuit pp(LOOP_RATE,true);
  ROS_INFO("set publisher...");

  // publish apollo wrapper topic 
  auto_msgs::control_cmd control_cmd_msg;
  ros::Publisher control_cmd_pub =nh.advertise<auto_msgs::control_cmd>("control_cmd", 3);
  //pub debug msg
  ros::Publisher pure_pursuit_debug_pub =nh.advertise
      <pure_pursuit_pkg::pure_pursuit_debug>("pure_pursuit_debug", 3);

  //subscribe apollo wrapper topic
  ros::Subscriber localizition_sub =
    nh.subscribe("localizition", 3, &pure_pursuit_pkg::PurePursuit::CBLocalizition, &pp);
  ros::Subscriber chassis_sub =
    nh.subscribe("chassis", 3, &pure_pursuit_pkg::PurePursuit::CBChassis, &pp);
  ros::Subscriber planning_sub =
    nh.subscribe("planning_path", 3, &pure_pursuit_pkg::PurePursuit::CBPlanning, &pp);
  
  ROS_INFO("pure pursuit start");
  ros::Rate loop_rate(LOOP_RATE);

  while (ros::ok())
  {
    ros::spinOnce();    
	  pp.step();
    pure_pursuit_pkg::pure_pursuit_debug temp_pure_pursuit_debug;
    temp_pure_pursuit_debug.later_dis_debug = pp.latral_displacement_;
    temp_pure_pursuit_debug.lookahead_distance_debug = pp.getLookaheadDistance();
    pure_pursuit_debug_pub.publish(temp_pure_pursuit_debug);

    //ControlCmdWrapper temp_control_cmd;
    auto_msgs::control_cmd temp_control_cmd;
    temp_control_cmd.throttle = pp.norAcc;
    temp_control_cmd.brake = pp.norBrake;
    double temp_angle;
    if(pp.tireAngleDeg*180/3.14>max_steering_wheel_angle)
    {
      temp_angle = max_steering_wheel_angle;
    }
    else if(pp.tireAngleDeg*180/3.14<-max_steering_wheel_angle)
    {
      temp_angle = -max_steering_wheel_angle;
    }
    else 
    {
      temp_angle = pp.tireAngleDeg*180/3.14;
    }
    temp_control_cmd.steering_target = temp_angle*STEERING_RATIO/max_steering_wheel_angle;//steering ratio is  18
    
    control_cmd_pub.publish(temp_control_cmd);
    loop_rate.sleep();
  }
  return 0;
}
